/**
 ******************************************************************************
 * @file    ins_task.c
 * @author  Wang Hongxi
 * @author  annotation and modificaiton by neozng
 * @version V2.0.0
 * @date    2022/2/23
 * @brief
 ******************************************************************************
 * @attention
 *
 ******************************************************************************
 */
#include "ins_task.h"
#include "controller.h"
#include "QuaternionEKF.h"
#include "bsp_spi.h"
#include "user_lib.h"
#include "general_def.h"
#include "arm_math.h"

#include "wit_process.h"
#include "dm_imu.h"


//#define IMU_BMI088
// #define IMU_WIT
#define IMU_DM

DM_IMU_Instance *dm_imu1;

static INS_t INS;
static IMU_Param_t IMU_Param;

const float xb[3] = {1, 0, 0};
const float yb[3] = {0, 1, 0};
const float zb[3] = {0, 0, 1};

// 用于获取两次采样之间的时间间隔
static uint32_t INS_DWT_Count = 0;
static float dt = 0, t = 0;

static uint8_t calling=1;           //未开始，所有的值都被记录在offset中

static float offset_yaw;

static void IMU_Param_Correction(IMU_Param_t *param, float gyro[3], float accel[3]);

void IMU_SetOffset()
{
    dm_imu1->measure.yaw_offset=dm_imu1->measure.yaw;
}

// 使用加速度计的数据初始化roll和pitch,而yaw置0,这样可以避免在初始时候的姿态估计误差
static void InitQuaternion(float *init_q4)
{
    float acc_init[3] = {0};
    float gravity_norm[3] = {0, 0, 1}; // 导航系重力加速度矢量,归一化后为(0,0,1)
    float axis_rot[3] = {0};           // 旋转轴
    // 读取100次加速度计数据,取平均值作为初始值
    for (uint8_t i = 0; i < 100; ++i)
    {
        BMI088_Read(&BMI088);
        acc_init[X] += BMI088.Accel[X];
        acc_init[Y] += BMI088.Accel[Y];
        acc_init[Z] += BMI088.Accel[Z];
        DWT_Delay(0.001);
    }
    for (uint8_t i = 0; i < 3; ++i)
        acc_init[i] /= 100;
    Norm3d(acc_init);
    // 计算原始加速度矢量和导航系重力加速度矢量的夹角
    float angle = acosf(Dot3d(acc_init, gravity_norm));
    Cross3d(acc_init, gravity_norm, axis_rot);
    Norm3d(axis_rot);
    init_q4[0] = cosf(angle / 2.0f);
    for (uint8_t i = 0; i < 2; ++i)
        init_q4[i + 1] = axis_rot[i] * sinf(angle / 2.0f); // 轴角公式,第三轴为0(没有z轴分量)
}

attitude_t *INS_Init(void)
{
    if (!INS.init)
        INS.init = 1;
    else
        return (attitude_t *)&INS.Gyro;

#ifdef IMU_BMI088
    while (BMI088Init(&hspi1, 0) != BMI088_NO_ERROR)
        ;
    IMU_Param.scale[X] = 1;
    IMU_Param.scale[Y] = 1;
    IMU_Param.scale[Z] = 1;
    IMU_Param.yaw = 0;
    IMU_Param.pitch = 0.0F;
    IMU_Param.roll = 0;
    IMU_Param.flag = 1;

    float init_quaternion[4] = {0};
    InitQuaternion(init_quaternion);
    IMU_QuaternionEKF_Init(init_quaternion, 10, 0.001, 1000000, 1, 0);

    // noise of accel is relatively big and of high freq,thus lpf is used
    INS.AccelLPF = 0.0085;
    DWT_GetDeltaT(&INS_DWT_Count);

#elifdef  IMU_WIT
    wit_init(&huart2);


#elifdef IMU_DM
    DM_IMU_Init_Config_s config = {.can_init_config =
    {.can_handle = &hcan1,
        .rx_id = 0x31,
        .tx_id = 0x41,}
    };
    dm_imu1 = DMIMUInit(&config);
#endif
    return (attitude_t *)&INS.Gyro; // @todo: 这里偷懒了,不要这样做! 修改INT_t结构体可能会导致异常,待修复.
}

/* 注意以1kHz的频率运行此任务 */
void INS_Task(void)
{
    static uint32_t count = 0;
    const float gravity[3] = {0, 0, 9.81f};

    dt = DWT_GetDeltaT(&INS_DWT_Count);
    t += dt;

    // ins update
    if ((count % 1) == 0)
    {
#ifdef IMU_BMI088
        BMI088_Read(&BMI088);

        INS.Accel[X] = BMI088.Accel[X];
        INS.Accel[Y] = BMI088.Accel[Y];
        INS.Accel[Z] = BMI088.Accel[Z];
        INS.Gyro[X] = BMI088.Gyro[X];
        INS.Gyro[Y] = BMI088.Gyro[Y];
        INS.Gyro[Z] = BMI088.Gyro[Z];

        // demo function,用于修正安装误差,可以不管,本demo暂时没用
        IMU_Param_Correction(&IMU_Param, INS.Gyro, INS.Accel);

        // 计算重力加速度矢量和b系的XY两轴的夹角,可用作功能扩展,本demo暂时没用
        // INS.atanxz = -atan2f(INS.Accel[X], INS.Accel[Z]) * 180 / PI;
        // INS.atanyz = atan2f(INS.Accel[Y], INS.Accel[Z]) * 180 / PI;

        // 核心函数,EKF更新四元数
        IMU_QuaternionEKF_Update(INS.Gyro[X], INS.Gyro[Y], INS.Gyro[Z], INS.Accel[X], INS.Accel[Y], INS.Accel[Z], dt);

        memcpy(INS.q, QEKF_INS.q, sizeof(QEKF_INS.q));

        // 机体系基向量转换到导航坐标系，本例选取惯性系为导航系
        BodyFrameToEarthFrame(xb, INS.xn, INS.q);
        BodyFrameToEarthFrame(yb, INS.yn, INS.q);
        BodyFrameToEarthFrame(zb, INS.zn, INS.q);

        // 将重力从导航坐标系n转换到机体系b,随后根据加速度计数据计算运动加速度
        float gravity_b[3];
        EarthFrameToBodyFrame(gravity, gravity_b, INS.q);
        for (uint8_t i = 0; i < 3; ++i) // 同样过一个低通滤波
        {
            INS.MotionAccel_b[i] = (INS.Accel[i] - gravity_b[i]) * dt / (INS.AccelLPF + dt) + INS.MotionAccel_b[i] * INS.AccelLPF / (INS.AccelLPF + dt);
        }
        BodyFrameToEarthFrame(INS.MotionAccel_b, INS.MotionAccel_n, INS.q); // 转换回导航系n

        INS.yaw = QEKF_INS.yaw;
        INS.pitch = QEKF_INS.pitch;
        INS.roll = QEKF_INS.roll;
        INS.yawTotalAngle = QEKF_INS.yawTotalAngle;

        // VisionSetAltitude(INS.yaw, INS.pitch, INS.roll);
#elifdef IMU_WIT

#elifdef IMU_DM
    INS.yaw = dm_imu1->measure.yaw;
    INS.pitch = dm_imu1->measure.pitch;
    INS.roll = dm_imu1->measure.roll;
    INS.yawTotalAngle = dm_imu1->measure.yawTotalAngle;

    DMIMURequestData(dm_imu1,UPDATE_ANGLE);
    // DMIMURequestData(dm_imu1,UPDATE_QUATERNION);
#endif
    }

    // temperature control
    if ((count % 2) == 0)
    {
        // 500hz

    }

    if ((count++ % 5) == 0)
    {
#ifdef IMU_BMI088

#elifdef IMU_WIT
        static uint32_t WIT_DWT_Count = 0;
        static float wit_dt ;
        wit_dt=DWT_GetDeltaT(&WIT_DWT_Count);
        static int32_t YawRoundCount;
        static float YawAngleLast;
        // 100Hz
        // wit_process();
        // WitReadReg(AX, 12);
        WitReadReg(Roll, 3);
        if(s_cDataUpdate)
        {
            if(calling!=0)
            {
                offset_yaw=sReg[Yaw] / 32768.0f * 180.0f;
            }
            YawAngleLast = INS.yaw;
            // for(int i = 0; i < 3; i++)
            // {
            //     INS.Accel[i] = sReg[AX+i] / 32768.0f * 16.0f;
            //     INS.Gyro[i] = sReg[GX+i] / 32768.0f * 2000.0f;
            // }
            INS.roll=sReg[Roll] / 32768.0f * 180.0f;
            INS.pitch=sReg[Pitch] / 32768.0f * 180.0f;
            INS.yaw=loop_float_constrain((sReg[Yaw] / 32768.0f * 180.0f)-offset_yaw,0,360.0);

            if (INS.yaw - YawAngleLast > 180.0f)
            {
                YawRoundCount--;
            }
            else if (INS.yaw - YawAngleLast < -180.0f)
            {
                YawRoundCount++;
            }

            INS.yawTotalAngle=(360.0f * YawRoundCount) + INS.yaw;

            s_cDataUpdate=0;
        }

#endif
    }

}

void INS_Start()
{
    calling=0;
}

/**
 * @brief          Transform 3dvector from BodyFrame to EarthFrame
 * @param[1]       vector in BodyFrame
 * @param[2]       vector in EarthFrame
 * @param[3]       quaternion
 */
void BodyFrameToEarthFrame(const float *vecBF, float *vecEF, float *q)
{
    vecEF[0] = 2.0f * ((0.5f - q[2] * q[2] - q[3] * q[3]) * vecBF[0] +
                       (q[1] * q[2] - q[0] * q[3]) * vecBF[1] +
                       (q[1] * q[3] + q[0] * q[2]) * vecBF[2]);

    vecEF[1] = 2.0f * ((q[1] * q[2] + q[0] * q[3]) * vecBF[0] +
                       (0.5f - q[1] * q[1] - q[3] * q[3]) * vecBF[1] +
                       (q[2] * q[3] - q[0] * q[1]) * vecBF[2]);

    vecEF[2] = 2.0f * ((q[1] * q[3] - q[0] * q[2]) * vecBF[0] +
                       (q[2] * q[3] + q[0] * q[1]) * vecBF[1] +
                       (0.5f - q[1] * q[1] - q[2] * q[2]) * vecBF[2]);
}

/**
 * @brief          Transform 3dvector from EarthFrame to BodyFrame
 * @param[1]       vector in EarthFrame
 * @param[2]       vector in BodyFrame
 * @param[3]       quaternion
 */
void EarthFrameToBodyFrame(const float *vecEF, float *vecBF, float *q)
{
    vecBF[0] = 2.0f * ((0.5f - q[2] * q[2] - q[3] * q[3]) * vecEF[0] +
                       (q[1] * q[2] + q[0] * q[3]) * vecEF[1] +
                       (q[1] * q[3] - q[0] * q[2]) * vecEF[2]);

    vecBF[1] = 2.0f * ((q[1] * q[2] - q[0] * q[3]) * vecEF[0] +
                       (0.5f - q[1] * q[1] - q[3] * q[3]) * vecEF[1] +
                       (q[2] * q[3] + q[0] * q[1]) * vecEF[2]);

    vecBF[2] = 2.0f * ((q[1] * q[3] + q[0] * q[2]) * vecEF[0] +
                       (q[2] * q[3] - q[0] * q[1]) * vecEF[1] +
                       (0.5f - q[1] * q[1] - q[2] * q[2]) * vecEF[2]);
}

/**
 * @brief reserved.用于修正IMU安装误差与标度因数误差,即陀螺仪轴和云台轴的安装偏移
 *
 *
 * @param param IMU参数
 * @param gyro  角速度
 * @param accel 加速度
 */
static void IMU_Param_Correction(IMU_Param_t *param, float gyro[3], float accel[3])
{
    static float lastyawOffset, lastpitchOffset, lastrollOffset;
    static float c_11, c_12, c_13, c_21, c_22, c_23, c_31, c_32, c_33;
    float cospitch, cosyaw, cosroll, sinpitch, sinyaw, sinroll;

    if (fabsf(param->yaw - lastyawOffset) > 0.001f ||
        fabsf(param->pitch - lastpitchOffset) > 0.001f ||
        fabsf(param->roll - lastrollOffset) > 0.001f || param->flag)
    {
        cosyaw = arm_cos_f32(param->yaw / 57.295779513f);
        cospitch = arm_cos_f32(param->pitch / 57.295779513f);
        cosroll = arm_cos_f32(param->roll / 57.295779513f);
        sinyaw = arm_sin_f32(param->yaw / 57.295779513f);
        sinpitch = arm_sin_f32(param->pitch / 57.295779513f);
        sinroll = arm_sin_f32(param->roll / 57.295779513f);

        // 1.yaw(alpha) 2.pitch(beta) 3.roll(gamma)
        c_11 = cosyaw * cosroll + sinyaw * sinpitch * sinroll;
        c_12 = cospitch * sinyaw;
        c_13 = cosyaw * sinroll - cosroll * sinyaw * sinpitch;
        c_21 = cosyaw * sinpitch * sinroll - cosroll * sinyaw;
        c_22 = cosyaw * cospitch;
        c_23 = -sinyaw * sinroll - cosyaw * cosroll * sinpitch;
        c_31 = -cospitch * sinroll;
        c_32 = sinpitch;
        c_33 = cospitch * cosroll;
        param->flag = 0;
    }
    float gyro_temp[3];
    for (uint8_t i = 0; i < 3; ++i)
        gyro_temp[i] = gyro[i] * param->scale[i];

    gyro[X] = c_11 * gyro_temp[X] +
              c_12 * gyro_temp[Y] +
              c_13 * gyro_temp[Z];
    gyro[Y] = c_21 * gyro_temp[X] +
              c_22 * gyro_temp[Y] +
              c_23 * gyro_temp[Z];
    gyro[Z] = c_31 * gyro_temp[X] +
              c_32 * gyro_temp[Y] +
              c_33 * gyro_temp[Z];

    float accel_temp[3];
    for (uint8_t i = 0; i < 3; ++i)
        accel_temp[i] = accel[i];

    accel[X] = c_11 * accel_temp[X] +
               c_12 * accel_temp[Y] +
               c_13 * accel_temp[Z];
    accel[Y] = c_21 * accel_temp[X] +
               c_22 * accel_temp[Y] +
               c_23 * accel_temp[Z];
    accel[Z] = c_31 * accel_temp[X] +
               c_32 * accel_temp[Y] +
               c_33 * accel_temp[Z];

    lastyawOffset = param->yaw;
    lastpitchOffset = param->pitch;
    lastrollOffset = param->roll;
}

//------------------------------------functions below are not used in this demo-------------------------------------------------
//----------------------------------you can read them for learning or programming-----------------------------------------------
//----------------------------------they could also be helpful for further design-----------------------------------------------

/**
 * @brief        Update quaternion
 */
void QuaternionUpdate(float *q, float gx, float gy, float gz, float dt)
{
    float qa, qb, qc;

    gx *= 0.5f * dt;
    gy *= 0.5f * dt;
    gz *= 0.5f * dt;
    qa = q[0];
    qb = q[1];
    qc = q[2];
    q[0] += (-qb * gx - qc * gy - q[3] * gz);
    q[1] += (qa * gx + qc * gz - q[3] * gy);
    q[2] += (qa * gy - qb * gz + q[3] * gx);
    q[3] += (qa * gz + qb * gy - qc * gx);
}

/**
 * @brief        Convert quaternion to eular angle
 */
void QuaternionToEularAngle(float *q, float *yaw, float *pitch, float *roll)
{
    *yaw = atan2f(2.0f * (q[0] * q[3] + q[1] * q[2]), 2.0f * (q[0] * q[0] + q[1] * q[1]) - 1.0f) * 57.295779513f;
    *pitch = atan2f(2.0f * (q[0] * q[1] + q[2] * q[3]), 2.0f * (q[0] * q[0] + q[3] * q[3]) - 1.0f) * 57.295779513f;
    *roll = asinf(2.0f * (q[0] * q[2] - q[1] * q[3])) * 57.295779513f;
}

/**
 * @brief        Convert eular angle to quaternion
 */
void EularAngleToQuaternion(float yaw, float pitch, float roll, float *q)
{
    float cospitch, cosyaw, cosroll, sinpitch, sinyaw, sinroll;
    yaw /= 57.295779513f;
    pitch /= 57.295779513f;
    roll /= 57.295779513f;
    cospitch = arm_cos_f32(pitch / 2);
    cosyaw = arm_cos_f32(yaw / 2);
    cosroll = arm_cos_f32(roll / 2);
    sinpitch = arm_sin_f32(pitch / 2);
    sinyaw = arm_sin_f32(yaw / 2);
    sinroll = arm_sin_f32(roll / 2);
    q[0] = cospitch * cosroll * cosyaw + sinpitch * sinroll * sinyaw;
    q[1] = sinpitch * cosroll * cosyaw - cospitch * sinroll * sinyaw;
    q[2] = sinpitch * cosroll * sinyaw + cospitch * sinroll * cosyaw;
    q[3] = cospitch * cosroll * sinyaw - sinpitch * sinroll * cosyaw;
}
